/*************************************************
 *  Object:       Tric! A flying Ricotta.
 *  Authors:      Teo (matteo.piva-gmail.com)
                  Carletto (carlo.checchi-gmail.com)
*************************************************/

#include "config.h"
#include "lib_bma180.h"
#include "lib_itg3200.h"
#include "sensors.h"


void SENSORS_init() {
    I2C1_Init(400000);
    Delay_ms(500);

    ITG3200_init();
    Delay_ms(500);
    BMA180_init();
    Delay_ms(500);
}

void SENSORS_read(float *SENSORS_acc, float *SENSORS_gyro) {
    BMA180_readAcc(&SENSORS_acc);
    ITG3200_readGyro(&SENSORS_gyro);
}

void SENSORS_process(float *SENSORS_acc, float *SENSORS_gyro, float *SENSORS_angle) {
    float vector;
    
    vector = sqrt(SENSORS_acc[0]*SENSORS_acc[0] + SENSORS_acc[1]*SENSORS_acc[1] + SENSORS_acc[2]*SENSORS_acc[2]);
    SENSORS_angle[0] = SENSORS_complementaryFilter(SENSORS_angle[0], asin(SENSORS_acc[1]/vector)*57.295779, -SENSORS_gyro[0]);
    SENSORS_angle[1] = SENSORS_complementaryFilter(SENSORS_angle[1], asin(SENSORS_acc[0]/vector)*57.295779, SENSORS_gyro[1]);
    SENSORS_angle[2] += (SENSORS_gyro[2]*SENSORS_processTime);
}

float SENSORS_complementaryFilter(float oldAngle, float newAngle, float newRate) {
    float filterK;
    float filteredAngle;
    
    filterK = SENSORS_filterTau / (SENSORS_filterTau + SENSORS_processTime);
    filteredAngle = filterK* (oldAngle + newRate * SENSORS_processTime) + (1-filterK) * (newAngle);
    return filteredAngle;
}